//---------------------------------------------------------------------------


#pragma hdrstop

#include "stdafx.h"
#include "MotionHMI.h"

//---------------------------------------------------------------------------

#pragma package(smart_init)

#define ADDINDEX_HRX(A, B) ((A + B) % HMI_RX_BUFF_SIZE)

unsigned char HMI_SendBackCheck[HMI_SBC_LENGTH] = {0xAA, 0xAA, 0xAA, 0xAA, 0xAA};
HMIRXStruct HMIRX = {0};
unsigned char HMI_Receive[HMI_R_LENGTH] = {0};

void HMI_Init(void)
{
	for(int i=0; i < HMI_R_LENGTH; i++)
	{
    HMI_Receive[i] = 0;
	}

	for(int i=0; i < HMI_RX_BUFF_SIZE; i++)
	{
    HMIRX.Buff[i] = 0;
  }
	HMIRX.NowIndex = 0;
	HMIRX.EndIndex = 0;
}

bool HMIRX2Receive(void)
{
  int num_nondealed_data = 0;
	int num_read_byte = 0;
	unsigned char checksum = 0;
	bool success = false;
	AnsiString str_show_rx = "HMI RX:";
	num_read_byte = HMI_PORT->Read(CommBuff, COMM_BUFF_SIZE);

	for(int i=0; i < num_read_byte; i++)
	{
		HMIRX.Buff[HMIRX.EndIndex] = CommBuff[i];
		HMIRX.EndIndex = ADDINDEX_HRX(HMIRX.EndIndex,1);
		#ifdef bShow_HMIRX
			if(bShow_HMIRX)
			{
				str_show_rx = str_show_rx + " " + IntToStr(CommBuff[i]);
			}
		#endif
	}
	#ifdef bShow_HMIRX
		if(bShow_HMIRX)
		{
			Memo_ShowInfo->Lines->Add(str_show_rx);
		}
	#endif

	if (HMIRX.NowIndex <= HMIRX.EndIndex)
	{
		num_nondealed_data = HMIRX.EndIndex - HMIRX.NowIndex;
	}
	else
	{
		num_nondealed_data = HMI_RX_BUFF_SIZE - (HMIRX.NowIndex - HMIRX.EndIndex);
	}

	while(num_nondealed_data >= HMI_R_LENGTH)
	{
		checksum = 0;
		for(int i=0; i<HMI_R_LENGTH-1; i++)
		{
			checksum += HMIRX.Buff[ADDINDEX_HRX(HMIRX.NowIndex,i)];
		}
		checksum = ~checksum;
		if(checksum == HMIRX.Buff[ADDINDEX_HRX(HMIRX.NowIndex,HMI_R_LENGTH-1)])
		{
			for(int i=0; i<HMI_R_LENGTH; i++)
			{
				HMI_Receive[i] = HMIRX.Buff[ADDINDEX_HRX(HMIRX.NowIndex,i)];

			}
			AccessHMIReceive();
      HMIRX.NowIndex = ADDINDEX_HRX(HMIRX.NowIndex,HMI_R_LENGTH);
			success = true;
		}
		else
		{
			HMIRX.NowIndex = ADDINDEX_HRX(HMIRX.NowIndex,1);
		}

		if (HMIRX.NowIndex <= HMIRX.EndIndex)
		{
			num_nondealed_data = HMIRX.EndIndex - HMIRX.NowIndex;
		}
		else
		{
			num_nondealed_data = HMI_RX_BUFF_SIZE - (HMIRX.NowIndex - HMIRX.EndIndex);
		}
	}
	return success;
}

void AccessHMIReceive(void)
{
	static TimeDifferenceStrcut T_HMI = {GetCPUTime(), 0, 0};
	AnsiString str_show_receive = "HMI Receive:";
	#ifdef bShow_HMIReceive
	if(bShow_HMIReceive)
	{
		for(int i=0; i<HMI_R_LENGTH; i++)
		{
			str_show_receive = str_show_receive + " " + IntToStr(HMI_Receive[i]);
		}
		Memo_ShowInfo->Lines->Add(str_show_receive);
	}
	#endif

	if( HMI_Receive[0] == 0xAA && HMI_Receive[1] == 0xAA &&
			HMI_Receive[2] == 0xAA && HMI_Receive[3] == 0xAA &&
			HMI_Receive[4] == 0xAA)
	{
		T_HMI.Now = GetCPUTime();
		CalculateTimeDifference_ms(&T_HMI);
		if(T_HMI.dT > 500)
		{
			CalculateReadChecksum();
      Comm_Write(HMI_PORT, &(MotorData.Read[0][0]), (MOTOR_NUM * MOTOR_READ_LENGTH));
    }
	}
	else if(HMI_Receive[INSTRUCTION_MODE] >= DEBUG_INST_BOUND ||
					HMI_Receive[INSTRUCTION_MODE] < SET_TORQUE)
	{
		Comm_Write(HMI_PORT, HMI_SendBackCheck, HMI_SBC_LENGTH);

		if(HMI_Receive[INSTRUCTION_MODE] == SCHEDULE_SWITCH)
		{
			bool enabled = (HMI_Receive[1] == 0) ? false : true;
			MotorScheduleSwitch(enabled);
			if(MotorSchedule.Enabled == false)
			{
				MotorSchedule.NowIndex = 0;
				MotorSchedule.EndIndex = 0;
      }
		}
		else if(HMI_Receive[INSTRUCTION_MODE] == REMOTE_HEAD_MODE)
		{
			Head.Mode_RX = HMI_Receive[1];
		}
		else if(HMI_Receive[INSTRUCTION_MODE] == REMOTE_HEAD_POSITION)
		{
			Add_head_job(0, HMI_Receive[1], HMI_Receive[2], 0, 1);
			Add_head_job(1, HMI_Receive[3], HMI_Receive[4], 0, 1);
		}
		else if(HMI_Receive[INSTRUCTION_MODE] == REMOTE_MOITON)
		{
			MotorScheduleSwitch(true);
			Motion.Mode_RX = HMI_Receive[1];
      if(Motion.Mode_RX == WALK)
			{
				if(HMI_Receive[2] == 1)
				{  // Walk_F
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_F_DISTANCE;
				}
				else if(HMI_Receive[2] == 2)
				{  // Walk_FR
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_FR_DISTANCE;
					Motion.ContinueAngle_RX = HMI_Receive[4] * WALK_FR_ANGLE;
				}
				else if(HMI_Receive[2] == 3)
				{  // Walk_FL
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_FL_DISTANCE;
					Motion.ContinueAngle_RX = HMI_Receive[4] * WALK_FL_ANGLE;
				}
				else if(HMI_Receive[2] == 4)
				{  // Walk_B
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_B_DISTANCE;
				}
				else if(HMI_Receive[2] == 5)
				{  // Walk_BR
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_BR_DISTANCE;
					Motion.ContinueAngle_RX = HMI_Receive[4] * WALK_BR_ANGLE;
				}
				else if(HMI_Receive[2] == 6)
				{  // Walk_BL
					Motion.ContinueDistance_RX = (HMI_Receive[3]) * WALK_BL_DISTANCE;
					Motion.ContinueAngle_RX = HMI_Receive[4] * WALK_BL_ANGLE;
				}
				else {
					Motion.ContinueDistance_RX = 0;
				 	Motion.ContinueAngle_RX = 0;
				}
			}
			else if(Motion.Mode_RX == SHIFT)
			{
				Motion.ContinueAngle_RX = 0;
				if(HMI_Receive[2] == 1)
					Motion.ContinueDistance_RX = HMI_Receive[4] * SHIFT_R_DISTANCE;
				else if(HMI_Receive[2] == 2)
					Motion.ContinueDistance_RX = HMI_Receive[4] * SHIFT_L_DISTANCE;
				else
					Motion.ContinueDistance_RX = 0;
			}
			else if(Motion.Mode_RX == TURN)
			{
				Motion.ContinueDistance_RX = 0;
				if(HMI_Receive[2] == 1)
					Motion.ContinueAngle_RX = HMI_Receive[4] * TURN_R_ANGLE;
				else if(HMI_Receive[2] == 2)
					Motion.ContinueAngle_RX = HMI_Receive[4] * TURN_L_ANGLE;
				else
					Motion.ContinueAngle_RX = 0;
			}
			else if(Motion.Mode_RX == TURNBALL)
			{
				Motion.ContinueDistance_RX = 0;
				if(HMI_Receive[2] == 1)
					Motion.ContinueAngle_RX = HMI_Receive[4] * TURNBALL_R_ANGLE;
				else if(HMI_Receive[2] == 2)
					Motion.ContinueAngle_RX = HMI_Receive[4] * TURNBALL_L_ANGLE;
				else
					Motion.ContinueAngle_RX = 0;
			}
			#ifdef THROW
			else if(Motion.Mode_RX == THROW)
			{
				if(HMI_Receive[2] == 0)
				{
					Motion.ContinueDistance_RX = 145;
				}
				else
				{
					Motion.ContinueDistance_RX = HMI_Receive[2];
				}
				Motion.ContinueAngle_RX = 0;
			}
			#endif
			#ifdef ROTATE_WAIST
			else if(Motion.Mode_RX == ROTATE_WAIST)
			{
				if(HMI_Receive[2] == 0)
				{
					Motion.ContinueAngle_RX = HMI_Receive[3];
				}
				else if(ZigbeeRX[2] == 1)
				{
					Motion.ContinueAngle_RX = -HMI_Receive[3];
				}
				else
				{
					Motion.ContinueAngle_RX = 0;
				}
				Motion.ContinueDistance_RX = 0;
			}
			#endif
			else
			{
				Motion.ContinueAngle_RX = 0;
				Motion.ContinueDistance_RX = 0;
			}

			#ifdef bShow_HMIRemote
				if(bShow_HMIRemote)
				{
					Memo_ShowInfo->Lines->Add("Remote motion: " + MotionMode2MotionName(Motion.Mode_RX) +
																		", " + IntToStr(Motion.Mode_RX) +
																		"; Distance" + FormatFloat("0.00", Motion.ContinueDistance_RX) +
																		", Angle" + FormatFloat("0.00", Motion.ContinueAngle_RX));
				}
			#endif
    } // End: else if(HMI_Receive[INSTRUCTION_MODE] == REMOTE_MOITON)
		else if(HMI_Receive[INSTRUCTION_MODE] == RETURN_MOTOR_READ)
		{
			int index = HMI_Receive[1];
			if(index >= 0 && index < MOTOR_NUM)
			{
				Comm_Write(HMI_PORT, &MotorData.Read[index][0], MOTOR_READ_LENGTH);
      }
    }
		else if(HMI_Receive[INSTRUCTION_MODE] == GET_MOTOR_DATA)
		{
			int id = HMI_Receive[1];
			int part, no;
			unsigned char send[3] = {0};

			if(IDtoPartNo(id, &part, &no) == true)
			{
				send[0] = id;
				send[1] = *(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + LOW_POSITION);
				send[2] = *(MotorAddress.Write[part] + (no * MOTOR_WRITE_LENGTH) + HIGH_POSITION);
				Comm_Write(HMI_PORT, send, 3);
			}
		}
		else if(HMI_Receive[INSTRUCTION_MODE] == GET_SCHEDULE_INDEX)
		{
			unsigned char send[2] = {MotorSchedule.NowIndex, MotorSchedule.EndIndex};
			Comm_Write(HMI_PORT, send, 2);
    }
		else if(HMI_Receive[INSTRUCTION_MODE] == GET_SCHEDULE_DATA)
		{
			unsigned char temp_now = (HMI_Receive[1] >= MOTOR_SCHEDULE_SIZE) ? (MOTOR_SCHEDULE_SIZE-1) : HMI_Receive[1];
			unsigned char temp_end = (HMI_Receive[2] >= MOTOR_SCHEDULE_SIZE) ? (MOTOR_SCHEDULE_SIZE-1) : HMI_Receive[2];
			int length = (temp_now - temp_end) * MOTOR_SCHEDULE_LIST_LENGTH;
			int i, j;

			if(length > 0)
			{
				Comm_Write(HMI_PORT, &MotorSchedule.List[i][0], length);
      }
		}
		else // if(HMI_Receive[INSTRUCTION_MODE] < SET TORQUE || )
		{
			// Including: HMI_Receive[INSTRUCTION_MODE] < SET TORQUE
			//						HMI_Receive[INSTRUCTION_MODE] == 253
			//						HMI_Receive[INSTRUCTION_MODE] == 255
			if(	(MotorSchedule.Enabled == true) ||
					(MotorSchedule.EndIndex == 0) ||
					((MotorSchedule.EndIndex > 0) && (MotorSchedule.List[MotorSchedule.EndIndex-1][0] != HMI_Receive[0])))
			{
				for(int i=0; i<MOTOR_SCHEDULE_LIST_LENGTH; i++)
				{
					MotorSchedule.List[MotorSchedule.EndIndex][i] =  HMI_Receive[i];
				}
				MotorSchedule.EndIndex = (MotorSchedule.EndIndex + 1) % MOTOR_SCHEDULE_SIZE;
				#ifdef bShow_Schedule
					if(bShow_Schedule)
					{
						AnsiString str_show = (MotorSchedule.Enabled) ? "ScheduleUpdate: ON" : "ScheduleUpdate: OFF";
						Memo_ShowInfo->Lines->Add(str_show + ", add job: " + IntToStr(HMI_Receive[0]));
					}
				#endif
			}
//			else if(MotorSchedule.EndIndex == MotorSchedule.NowIndex)
//			{
//				#ifdef bShow_Schedule
//					if(bShow_Schedule)
//					{
//            AnsiString str_show = "";
//						Memo_ShowInfo->Lines->Add("Fail to Add job; NowIndex: " + IntToStr(MotorSchedule.NowIndex) +
//																			"EndIndex, " + IntToStr(MotorSchedule.EndIndex));
//						if(MotorSchedule.EndIndex > 0)
//						{
//							Memo_ShowInfo->Lines->Add("Last job:" + IntToStr(MotorSchedule.List[MotorSchedule.EndIndex-1][0]) +
//																				", new job: " + IntToStr(HMI_Receive[0]));
//						}
//						else
//						{
//							Memo_ShowInfo->Lines->Add("Last job:" + IntToStr(MotorSchedule.List[MOTOR_SCHEDULE_SIZE-1][0]) +
//																				", new job: " + IntToStr(HMI_Receive[0]));
//						}
//						Memo_ShowInfo->Lines->Add("Flush schedule.\r\n");
//					}
//				#endif
//				MotorSchedule.NowIndex = 0;
//				MotorSchedule.EndIndex = 0;
//        for(int i=0; i < MOTOR_SCHEDULE_SIZE; i++)
//				{
//					for(int j=0; j < MOTOR_SCHEDULE_LIST_LENGTH; j++)
//					{
//						MotorSchedule.List[i][j] = 0;
//					}
//				}
//      }
		}
  }
}

void HMI_StoreToShareM(void)
{

}

void HMI_LoadFromShareM(void)
{
	while(Mem_Info->HMI.Send_SIM_Now != Mem_Info->HMI.Send_SIM_End &&
				Mem_Info->HMI.Send)
	{
		HMI_Receive[0] = Mem_Info->HMI.Send_SIM[Mem_Info->HMI.Send_SIM_Now][0];
		HMI_Receive[1] = Mem_Info->HMI.Send_SIM[Mem_Info->HMI.Send_SIM_Now][1] & 0x00ff;
		HMI_Receive[2] = (Mem_Info->HMI.Send_SIM[Mem_Info->HMI.Send_SIM_Now][1] & 0xff00) >> 8;
		HMI_Receive[3] = Mem_Info->HMI.Send_SIM[Mem_Info->HMI.Send_SIM_Now][2] & 0x00ff;
		HMI_Receive[4] = (Mem_Info->HMI.Send_SIM[Mem_Info->HMI.Send_SIM_Now][2] & 0xff00) >> 8;
		Mem_Info->HMI.Send_SIM_Now = (Mem_Info->HMI.Send_SIM_Now + 1) % BUFFER_NUM;
		AccessHMIReceive();
	}
	if(Mem_Info->HMI.Send_SIM_Now == Mem_Info->HMI.Send_SIM_End)
	{
		Mem_Info->HMI.Send_SIM_Now = 0;
		Mem_Info->HMI.Send_SIM_End = 0;
		Mem_Info->HMI.Send = false;
	}
}

